#!/usr/bin/env python2
# -*- coding: utf-8 -*-

"""
Brief  : 书法机器人上位机
Author : 傅帅
Date   : 2020.7.15
"""
import Tkinter as tk
import tkFont
import tkMessageBox
import tkFileDialog
from Tkinter import NW
from PIL import Image, ImageTk

import os
import re
import json
import copy
import time
import rospy
import threading
import numpy as np
from math import *
from misc import config
from elfin_driver import ElfinDriver, TransMartrix2xyzrpy, xyzrpy2TransMartrix
from elfin_driver import TransMartrix2xyzrpy, xyzrpy2TransMartrix
from stroke_creator import StrokeCreator
from font_creator import OpenFontCreator
from record_creator import RecordCreator

root = tk.Tk()
# 字体
black12 = tkFont.Font(root, family="黑体", size=12)
black14 = tkFont.Font(root, family="黑体", size=14)
black16 = tkFont.Font(root, family="黑体", size=16)
# 图片相关
pics = []  # 这个用于存储最后用于画在canvas上的PhotoImage类型的对象，防止其作为局部变量被回收，canvas画img的大坑-_-|.......
canvas = tk.Canvas()
canvas_menu = tk.Menu()
canvas_mouse_state = 'up'  # 'up'/'select'/'drag'
canvas_mouse_down_pos = [] # 鼠标左键按下时的坐标
choose_box_canvas_id = -1 # 选择框的canvas_id
selected_canvas_ids = []  # 存储当前选择的对象的canvas_id
canvas_ctrl_down = False  # 标记左键抬起时是否按下了Ctrl
BORDER_SIZE = 2  # 选中框的宽度
FENCE_GAP = config.FENCE_GAP  # 栅格间距
drag_box_canvas_ids = []  # 存储拖动框的canvas_id
# 动态控件
coord_x_entry = tk.Entry
coord_y_entry = tk.Entry
coord_z_entry = tk.Entry
coord_theta_entry = tk.Entry
working_state = tk.StringVar
working_state_label = tk.Label
write_btn = tk.Button
# 工作空间(宣纸)尺寸[mm]
WORK_SPACE_WIDTH = 1000.0
WORK_SPACE_HEIGHT = 550.0
# 画布尺寸[pix]
MM_PER_PIX = 1  # 此值后面直接默认为1了,因为要变动的话非常麻烦也没啥大用
CANVAS_WIDTH = WORK_SPACE_WIDTH / MM_PER_PIX
CANVAS_HEIGHT = WORK_SPACE_HEIGHT / MM_PER_PIX
# 完全表示当前工作空间的大字典
current_workspace = {
    'items':[],
    # 'items':[
    #     {
    #         'name':'',
    #         'position':[x,y],
    #         'rectangle_size':[x,y],
    #         'type':'font'/'stroke'
    #         'canvas_id':id
    #     },
    #     {
    #         ...
    #     }
    # ],
    'params':{'coord_x':0.0,'coord_y':0.0,'coord_z':0.0,'coord_theta':0.0}
}
# elfin_ros相关
rospy.init_node('calligraphy_robot_controller', anonymous=True)
elfin_driver = ElfinDriver()
writing_state = 'IDLE'  # 'IDLE'/'MOVING'/'WRITING'/'PAUSE'
writing_index = 0       # 标志接下来要写哪一段
calculated_paths = []
# calculated_paths = 
# [
#   {
#     'path_points':[[x, y, z, raw, yaw, pitch],...],
#     'canvas_rectangle':[[NWx,NWy], [SEx,SEy]]  # 该笔画在画布上的矩形区域
#   },
#   {
#     ...
#   }
# ]

def GUI_Init():
    global canvas, canvas_menu, coord_x_entry, coord_y_entry, coord_z_entry, coord_theta_entry, \
        working_state, working_state_label, write_btn
    """GUI布局初始化"""
    root.title('CalligraphyRobot')
    root.geometry("1100x840")
    root.resizable(width = False, height = False)
    # ---------------------------------- 菜单 ----------------------------------------
    menubar = tk.Menu(root, font=black16)
    createmenu = tk.Menu(menubar, tearoff=False, font=black12)
    menubar.add_cascade(label="创建", menu=createmenu)
    createmenu.add_command(label="创建笔画", command=CreateStroke)
    createmenu.add_command(label="创建文字", command=CreateFont)
    createmenu.add_command(label="创建动捕记录", command=CreateRecord)

    addmenu = tk.Menu(menubar, tearoff=False, font=black12)
    menubar.add_cascade(label="添加", menu=addmenu)
    addmenu.add_command(label="加入笔画", command=AddStroke)
    addmenu.add_command(label="加入文字", command=AddFont)
    addmenu.add_command(label="加入动捕记录", command=AddRecord)

    savemenu = tk.Menu(menubar, tearoff=False, font=black12)
    menubar.add_cascade(label="保存", menu=savemenu)
    savemenu.add_command(label="保存当前工作空间", command=SaveWorkSpace)

    loadmenu = tk.Menu(menubar, tearoff=False, font=black12)
    menubar.add_cascade(label="导入", menu=loadmenu)
    loadmenu.add_command(label="导入工作空间", command=LoadWorkSpace)
    # 将menubar设置为顶部菜单
    root.config(menu=menubar)
    # ---------------------------------- 画布 ----------------------------------------
    # 创建画布
    canvas = tk.Canvas(root, bg = "white", width = CANVAS_WIDTH, height = CANVAS_HEIGHT)
    canvas.place(x=50, y=30)
    # 绑定鼠标事件
    canvas.bind('<Button-1>', onCanvasLeftButtonDown)
    canvas.bind('<B1-Motion>', onCanvasLeftButtonMove)
    canvas.bind('<ButtonRelease-1>', onCanvasLeftButtonUp)
    canvas.bind('<ButtonRelease-3>', onCanvasRightButtonUp)
    canvas.bind("<Control-ButtonRelease-1>", onCtrlCanvasLeftButtonUp)
    # 画布下的菜单
    canvas_menu = tk.Menu(canvas, tearoff=0)
    canvas_menu.add_command(label='删除所选', command=DeleteSelectionBtnCb, font=black14)
    canvas_menu.add_command(label='清空工作空间', command=ClearWorkspaceBtnCb, font=black14)
    # 虚线(用于排序)
    _DrawFence()
    # ---------------------------------- 控制面板 ----------------------------------------
    # -------------------- 容器LabelFrame ----------------------
    panel_frame = tk.LabelFrame(root, text="控制面板", labelanchor="nw", font=black16)
    panel_frame['width'] = 950
    panel_frame['height'] = 220
    panel_frame.place(x=70, y=600)
    # ----------------------- 标签 -----------------------------
    # 静态标签
    tk.Label(panel_frame, text="工作状态:", font=black14, fg='black').place(x=50, y=10)
    tk.Label(panel_frame, text="设置坐标系变换关系(机械臂基坐标系到宣纸左上角为零点的右手系)", 
            font=black14, fg='black').place(x=50, y=45)
    tk.Label(panel_frame, text="x[mm]:", font=black14, fg='black').place(x=50, y=80)
    tk.Label(panel_frame, text="y[mm]:", font=black14, fg='black').place(x=250, y=80)
    tk.Label(panel_frame, text="z[mm]:", font=black14, fg='black').place(x=460, y=80)
    tk.Label(panel_frame, text="theta[degree]:", font=black14, fg='black').place(x=660, y=80)
    # 动态标签
    working_state = tk.StringVar(panel_frame, "未开始书写")
    working_state_label = tk.Label(panel_frame, textvariable=working_state, font=black14, fg='red')
    working_state_label.place(x=150, y=10)
    # ----------------------- 输入框 -----------------------------
    coord_x_entry = tk.Entry(panel_frame, width='10', font=black12)
    coord_x_entry.place(x=120, y=83)
    coord_x_entry.insert(0, 0.0)
    coord_y_entry = tk.Entry(panel_frame, width='10', font=black12)
    coord_y_entry.place(x=320, y=83)
    coord_y_entry.insert(0, 0.0)
    coord_z_entry = tk.Entry(panel_frame, width='10', font=black12)
    coord_z_entry.place(x=530, y=83)
    coord_z_entry.insert(0, 0.0)
    coord_theta_entry = tk.Entry(panel_frame, width='10', font=black12)
    coord_theta_entry.place(x=800, y=83)
    coord_theta_entry.insert(0, 0.0)
    # ----------------------- 按钮 -----------------------------
    calculate_btn = tk.Button(panel_frame, text="轨迹计算", command=CalculateBtnCb, font=black12)
    calculate_btn.place(x=100, y=130)
    write_btn = tk.Button(panel_frame, text="开始书写", command=WriteBtnCb, font=black12)
    write_btn.place(x=300, y=130)
    stop_btn = tk.Button(panel_frame, text="停止动作", command=StopBtnCb, font=black12)
    stop_btn.place(x=500, y=130)
    home_btn = tk.Button(panel_frame, text="返回零位", command=HomeBtnCb, font=black12)
    home_btn.place(x=700, y=130)
    tk.Button(panel_frame, text="取笔", command=GetPenBtnCb, font=black12).place(x=640, y=4)
    tk.Button(panel_frame, text="蘸墨", command=InkBtnCb, font=black12).place(x=740, y=4)
    tk.Button(panel_frame, text="收笔", command=SetPenBtnCb, font=black12).place(x=840, y=4)
    # ---------------------------------- 启动 ----------------------------------------
    root.protocol("WM_DELETE_WINDOW", OnClose)  # 注册关闭事件
    _LoadWorkspace('./misc/default_workspace.json')  # 导入保存的配置
    root.mainloop()

def CreateStroke():
    '''弹出窗口.用于新建笔画,保存参数文件json和预览图png'''
    stroke_creator_window = tk.Toplevel()
    StrokeCreator(stroke_creator_window)

def CreateFont():
    '''弹出窗口.用于组装笔画,新建文字,保存参数文件json和预览图png'''
    font_creator_window = tk.Toplevel()
    OpenFontCreator(font_creator_window)

def CreateRecord():
    '''弹出窗口.用于新建动捕记录,保存参数文件json和预览图png'''
    record_creator_window = tk.Toplevel()
    RecordCreator(record_creator_window)

def AddStroke():
    '''弹出窗口,添加笔画'''
    add_stroke_window = tk.Toplevel()
    AddItemWindow(add_stroke_window, 'stroke')

def AddFont():
    '''弹出窗口,添加文字'''
    add_font_window = tk.Toplevel()
    AddItemWindow(add_font_window, 'font')

def AddRecord():
    '''弹出窗口,添加动捕记录'''
    add_font_window = tk.Toplevel()
    AddItemWindow(add_font_window, 'record')

def SaveWorkSpace():
    path = tkFileDialog.asksaveasfilename(title='保存工作空间', filetypes=[('工作空间记录文件', '*.json')])
    if path == '' or path == (): return  # 未选择便退出文件对话框
    if _SaveWorkspace(path):
        tkMessageBox.showinfo('提示', '工作空间保存成功', parent=root)

def LoadWorkSpace():
    path = tkFileDialog.askopenfilename(title='保存工作空间', filetypes=[('工作空间记录文件', '*.json')])
    if path == '' or path == (): return  # 未选择便退出文件对话框
    if _LoadWorkspace(path):
        tkMessageBox.showinfo('提示', '工作空间导入成功', parent=root)

def CalculateBtnCb():
    '''
    根据current_workspace中的状态，整理并计算出用于控制机械臂的calculated_paths
    此命令会直接从json文件重新读取各个笔画的参数，所以调整新笔画时,可在更新笔画json后直接重新计算然后测试
    如此便可不必在设计窗口再提供计算和测试功能了
    '''
    global calculated_paths
    calculated_paths = []
    # 先对current_workspace['items']进行排序,从右到左,从上到下
    dashX = CANVAS_WIDTH - FENCE_GAP  # X方向排序以虚线为准
    tmp_items = []  # 存最终排序结果
    while dashX > -FENCE_GAP:
        tmp_list = []  # 对每一块X区域，用tmp_list冒泡排Y
        for item in current_workspace['items']:
            if dashX <= item['position'][0] < dashX+FENCE_GAP:  # X在该区域内
                _BubbleSorting(item, tmp_list)
        for _item in tmp_list:  # 将该块X区域排序结果送入最终排序结果
            tmp_items.append(_item)
        dashX -= FENCE_GAP
    current_workspace['items'] = tmp_items  # 将排序好的列表更新给items
    # 整理和坐标变换,计算calculated_paths
    for item in current_workspace['items']:
        obj = json.loads(open('./calligraphy_lib/'+item['type']+'s/'+item['name']+'.json','r').read())
        if item['type'] == 'stroke':
            org_points = np.array(obj['stroke_points'])  # 原始点集
            _position = copy.deepcopy(item['position'])
            if len(org_points[0]) == 3:  # 笔画数据无rpy,为手动创建的笔画
                org_points *= [1,1,-config.BRUSH_HEIGHT]  # 将归一化的Z轴0to1换算为mm
                _position.append(0)
                canvas_points = org_points + _position  # 相对画布左上角点集
                paper_coord_points = canvas_points * [1, -1, 1]  # 宣纸左上角零点右手系坐标
            else:  # 笔画数据带rpy,为记录生成的笔画
                for i in range(4):
                    _position.append(0)
                canvas_points = org_points + _position
                paper_coord_points = canvas_points * [1, -1, 1, 1, 1, 1]
            path_points = CoordTrans(paper_coord_points)
            calculated_paths.append({
                'path_points':path_points, 
                'canvas_rectangle':[
                    [item['position'][0], item['position'][1]],
                    [item['position'][0]+item['rectangle_size'][0], item['position'][1]+item['rectangle_size'][1]]
                ]
            })
        elif item['type'] == 'font':
            # 依次将每一个笔画加入
            for stroke in obj['strokes']:
                stroke_name = stroke['stroke_name']
                stroke_offset = copy.deepcopy(stroke['stroke_offset'])
                stroke_obj = json.loads(open('./calligraphy_lib/strokes/'+stroke_name+'.json','r').read())
                stroke_size = [int(stroke_obj['stroke_size'][0]),int(stroke_obj['stroke_size'][1])]
                org_points = np.array(stroke_obj['stroke_points'])
                _position = copy.deepcopy(item['position'])
                if len(org_points[0]) == 3:  # 笔画数据无rpy,为手动创建的笔画
                    org_points *= [1,1,config.BRUSH_HEIGHT]  # 将归一化的Z轴-1to1换算为mm
                    _position.append(0)
                    stroke_offset.append(0)
                    canvas_points = org_points + _position + stroke_offset
                    paper_coord_points = canvas_points * [1, -1, 1]
                else:  # 笔画数据带rpy,为记录生成的笔画
                    for i in range(4):
                        _position.append(0)
                        stroke_offset.append(0)
                    canvas_points = org_points + _position + stroke_offset
                    paper_coord_points = canvas_points * [1, -1, 1, 1, 1, 1]
                path_points = CoordTrans(paper_coord_points)
                calculated_paths.append({
                    'path_points':path_points, 
                    'canvas_rectangle':[
                        [item['position'][0]+stroke_offset[0], item['position'][1]+stroke_offset[1]],
                        [item['position'][0]+stroke_offset[0]+stroke_size[0], item['position'][1]+stroke_offset[1]+stroke_size[1]]
                    ]
                })
        elif item['type'] == 'record':
            org_points = np.array(obj['record_points'])  # 原始点集
            _position = copy.deepcopy(item['position'])
            for i in range(4):
                _position.append(0)
            canvas_points = org_points + _position
            paper_coord_points = canvas_points * [1, -1, 1, 1, 1, 1]
            path_points = CoordTrans(paper_coord_points)
            calculated_paths.append({
                'path_points':path_points, 
                'canvas_rectangle':[
                    [item['position'][0], item['position'][1]],
                    [item['position'][0]+item['rectangle_size'][0], item['position'][1]+item['rectangle_size'][1]]
                ]
            })
    # 画图，展示calculated_paths书写顺序
    for index, path in enumerate(calculated_paths):
        rect = path['canvas_rectangle']
        canvas.create_rectangle(rect[0][0]-BORDER_SIZE, rect[0][1]-BORDER_SIZE,
            rect[1][0]+BORDER_SIZE, rect[1][1]+BORDER_SIZE, outline='red', width=BORDER_SIZE)
        canvas.create_oval(rect[1][0]+4, rect[0][1]-26, rect[1][0]+26, rect[0][1]-4, outline='red', width=BORDER_SIZE)
        if index+1 < 10:
            canvas.create_text(rect[1][0]+10, rect[0][1]-30, text=str(index+1), font=black14, fill='red', anchor=NW)
        else:
            canvas.create_text(rect[1][0]+7, rect[0][1]-28, text=str(index+1), font=black12, fill='red', anchor=NW)
    StopBtnCb()

def WriteBtnCb():
    writing_thread = threading.Thread(target=Write, args=())
    writing_thread.daemon = True
    writing_thread.start()

def Write():
    '''控制机械臂抬笔落笔，依次遍历calculated_paths中的路径'''
    global working_state, working_state_label, writing_state, writing_index
    paths_to_write = copy.deepcopy(calculated_paths)[writing_index:]
    for path in paths_to_write:
        # 移动到位
        writing_state = 'MOVING'
        HEIGHT = 20  # 抬笔高度[mm]
        # 自动规划到起始点
        target_pos = np.array(path['path_points'][0]) + [0,0,HEIGHT,0,0,0]
        elfin_driver.control_position(list(target_pos))  # 发布控制指令
        working_state.set('正在移动到第'+str(writing_index+1)+'段轨迹起点')
        working_state_label['fg'] = 'green'
        write_btn['text'] = '暂停书写'
        write_btn['fg'] = 'red'
        write_btn['command'] = PauseBtnCb
        # 轮询判断是否运动已结束或暂停/停止按钮被按下
        xyzWINDOW = 1  # mm
        rpyWINDOW = 0.02  # rad
        while True:
            # 判断是否到达
            if sum(abs(target_pos[0:3] - elfin_driver.TCP_pos[0:3])) < xyzWINDOW \
            and sum(abs(abs(target_pos[3:]) - abs(np.array(elfin_driver.TCP_pos[3:])))) < rpyWINDOW:
                time.sleep(0.2)  # 防止下条指令过快发出
                if writing_state != 'MOVING': return
                break
            if writing_state != 'MOVING':
                return
            time.sleep(0.1)
        # 落笔写字
        writing_state = 'WRITING'
        write_path = np.array(path['path_points'])
        write_path = np.append(write_path, write_path[-1] + [0,0,HEIGHT,0,0,0]).reshape((-1,6))  # 添加一点进行抬笔
        elfin_driver.control_path(write_path)  # 发布控制指令
        working_state.set('正在书写第'+str(writing_index+1)+'段轨迹')
        working_state_label['fg'] = 'blue'
        # 轮询判断是否运动已结束或暂停/停止按钮被按下
        while True:
            if sum(abs(write_path[-1][0:3] - elfin_driver.TCP_pos[0:3])) < xyzWINDOW \
            and sum(abs(abs(write_path[-1][3:]) - abs(np.array(elfin_driver.TCP_pos[3:])))) < rpyWINDOW:
                writing_index += 1
                time.sleep(0.2)  # 防止下条指令过快发出
                if writing_state != 'WRITING': return
                break
            if writing_state != 'WRITING':
                return
            time.sleep(0.1)
    working_state.set('书写已结束!')
    working_state_label['fg'] = 'green'
    write_btn['text'] = '开始书写'
    write_btn['fg'] = 'black'
    write_btn['command'] = WriteBtnCb
    writing_index = 0
    writing_state = 'IDLE'

def PauseBtnCb():
    global writing_index, working_state, working_state_label, writing_state, write_btn
    if writing_state == 'WRITING': writing_index += 1  # 若在MOVING中暂停，则不需切至下一段
    working_state.set('已暂停，准备书写第'+str(writing_index+1)+'段轨迹')
    working_state_label['fg'] = 'red'
    writing_state = 'PAUSE'
    write_btn['text'] = '继续书写'
    write_btn['fg'] = 'green'
    write_btn['command'] = WriteBtnCb
    tkMessageBox.showinfo('提示', '待当前动作结束后自动暂停', parent=root)

def StopBtnCb():
    global working_state, working_state_label, writing_state, write_btn, writing_index
    working_state.set('未开始书写')
    working_state_label['fg'] = 'red'
    writing_state = 'IDLE'
    write_btn['text'] = '开始书写'
    write_btn['fg'] = 'black'
    write_btn['command'] = WriteBtnCb
    writing_index = 0
    elfin_driver.stop()

def HomeBtnCb():
    global working_state, working_state_label, writing_state, write_btn, writing_index
    working_state.set('未开始书写')
    working_state_label['fg'] = 'red'
    writing_state = 'IDLE'
    write_btn['text'] = '开始书写'
    write_btn['fg'] = 'black'
    write_btn['command'] = WriteBtnCb
    writing_index = 0
    elfin_driver.home()

def GetPenBtnCb():
    if writing_state!='IDLE': tkMessageBox.showinfo("错误",'应在停止状态下取笔', parent=root);return
    motion_thread = threading.Thread(target=PenMotion, args=('get',))
    motion_thread.daemon = True
    motion_thread.start()

def InkBtnCb():
    if writing_state!='IDLE' and writing_state!='PAUSE': tkMessageBox.showinfo("错误",'应在停止或暂停状态下蘸墨', parent=root);return
    motion_thread = threading.Thread(target=PenMotion, args=('ink',))
    motion_thread.daemon = True
    motion_thread.start()

def SetPenBtnCb():
    if writing_state!='IDLE': tkMessageBox.showinfo("错误",'应在停止状态下放笔', parent=root);return
    motion_thread = threading.Thread(target=PenMotion, args=('set',))
    motion_thread.daemon = True
    motion_thread.start()

def PenMotion(motion_type):
    global writing_state, working_state
    initial_pos = elfin_driver.TCP_pos
    initial_state = writing_state
    writing_state = 'MOVING'  # 置书写状态为MOVING
    if motion_type == 'get':
        SHELF_X_OFFSET = -20  # 第一步离笔架的距离
        working_state.set(working_state.get()+'(取笔中)')
        # 到取笔第一个点
        shelf_pos = [SHELF_X_OFFSET, 0, 0, pi, 0, 0]
        TCP_pos = _PenShelfTrans(shelf_pos)
        elfin_driver.control_position(TCP_pos)
        if _ArriveJudge(TCP_pos) == -1: return  # 等待到达，被打断则return
        time.sleep(1)
        # 到取笔第二个点
        shelf_pos = [0, 0, 0, pi, 0, 0]
        TCP_pos = _PenShelfTrans(shelf_pos)
        elfin_driver.control_position(TCP_pos)
        if _ArriveJudge(TCP_pos) == -1: return  # 等待到达，被打断则return
        time.sleep(4)  # 此时软体手夹紧
        # 到取笔第三个点
        shelf_pos = [SHELF_X_OFFSET, 0, 10, pi, 0, 0]
        TCP_pos = _PenShelfTrans(shelf_pos)
        elfin_driver.control_position(TCP_pos)
        if _ArriveJudge(TCP_pos) == -1: return  # 等待到达，被打断则return
        # 回初始点
        elfin_driver.control_position(initial_pos)
        if _ArriveJudge(initial_pos) == -1: return  # 等待到达，被打断则return
    # 取笔放笔流程基本一致
    elif motion_type == 'set':  
        SHELF_X_OFFSET = -20  # 第一步离笔架的距离
        working_state.set(working_state.get()+'(放笔中)')
        # 到放笔第一个点
        shelf_pos = [SHELF_X_OFFSET, 0, 10, pi, 0, 0]
        TCP_pos = _PenShelfTrans(shelf_pos)
        elfin_driver.control_position(TCP_pos)
        if _ArriveJudge(TCP_pos) == -1: return  # 等待到达，被打断则return
        time.sleep(1)
        # 到放笔第二个点
        shelf_pos = [0, 0, 0, pi, 0, 0]
        TCP_pos = _PenShelfTrans(shelf_pos)
        elfin_driver.control_position(TCP_pos)
        if _ArriveJudge(TCP_pos) == -1: return  # 等待到达，被打断则return
        time.sleep(4)  # 此时软体手松开
        # 到放笔第三个点
        shelf_pos = [SHELF_X_OFFSET, 0, 0, pi, 0, 0]
        TCP_pos = _PenShelfTrans(shelf_pos)
        elfin_driver.control_position(TCP_pos)
        if _ArriveJudge(TCP_pos) == -1: return  # 等待到达，被打断则return
        # 回初始点
        elfin_driver.control_position(initial_pos)
        if _ArriveJudge(initial_pos) == -1: return  # 等待到达，被打断则return
    elif motion_type == 'ink':
        HEIGHT = 50.0  # 前往砚台上空高度
        working_state.set(working_state.get()+'(蘸墨中)')
        # 蘸墨时默认X轴方向和宣纸正向一致
        # 第一点,前往砚台上方
        end_pos = [config.INK_PARAMS[0], config.INK_PARAMS[1], config.INK_PARAMS[2]+HEIGHT,\
                   pi, 0, float(coord_theta_entry.get())/180.0*pi]
        TCP_pos = _End2TCP(end_pos)
        elfin_driver.control_position(TCP_pos)
        if _ArriveJudge(TCP_pos) == -1: return  # 等待到达，被打断则return
        time.sleep(1)
        # 第二段,执行蘸墨轨迹
        end_pos = [config.INK_PARAMS[0], config.INK_PARAMS[1], config.INK_PARAMS[2],\
                   pi, 0, float(coord_theta_entry.get())/180.0*pi]
        offset_array = [[0,0,0],  # 蘸墨轨迹的偏置数组,可在此调节蘸墨轨迹,xyz对应坐标系为基座标系
            [-20, 0, 0],
            [20, 0, 0],
            [0, 0, 0],
            [0, 20, 0],
            [0, -20, 0],
            [46, 0, 22],
            [-30, 0, 30],
            [-30, 0, 10],
            [-46, 0, 22],
            [30, 0, 30],
            [30, 0, 10],
            [46, 0, 22],
            [0, 0, HEIGHT],
        ]
        TCP_path = []
        for i in range(len(offset_array)):
            _end_pos = [end_pos[0]+offset_array[i][0],end_pos[1]+offset_array[i][1],end_pos[2]+offset_array[i][2],\
                        end_pos[3],end_pos[4],end_pos[5]]
            TCP_path.append(_End2TCP(_end_pos))
        elfin_driver.control_path(TCP_path)
        if _ArriveJudge(TCP_path[-1]) == -1: return  # 等待到达，被打断则return
        time.sleep(2)
        # 回初始点
        elfin_driver.control_position(initial_pos)
        if _ArriveJudge(initial_pos) == -1: return  # 等待到达，被打断则return
    # 置回进入时状态
    writing_state = initial_state
    working_state.set(working_state.get()[:-5])

def DeleteSelectionBtnCb():
    global current_workspace, selected_canvas_ids
    '''从current_workspace中删除掉selected_canvas_ids'''
    delete_list = []
    for index, item in enumerate(current_workspace['items']):
        if item['canvas_id'] in selected_canvas_ids:
            delete_list.append(index)
    delete_list.reverse()  # 从后往前删
    for index in delete_list:
        current_workspace['items'].pop(index)
    selected_canvas_ids = []
    UpdateCanvas()

def ClearWorkspaceBtnCb():
    global current_workspace, selected_canvas_ids
    current_workspace['items'] = []
    selected_canvas_ids = []
    UpdateCanvas()

def OnClose():
    _SaveWorkspace('./misc/default_workspace.json')
    root.destroy()

def onCanvasLeftButtonDown(event):
    global canvas_mouse_state,canvas_mouse_down_pos
    canvas_mouse_down_pos = [event.x, event.y]
    canvas_menu.unpost()
    # 根据按下位置是否在已选中范围内判断select or drag
    for canvas_id in selected_canvas_ids:  # 遍历已选中对象
        for item in current_workspace['items']:  # 获取该canvas_id的对象
            if item['canvas_id'] == canvas_id:
                break
        if _AInBRange([event.x, event.y], item['position'], item['rectangle_size']):
            # 若检测到按键位置处于任意一个已选中范围内,鼠标状态设置为拖动
            canvas_mouse_state = 'drag'
            return
    # 若不在任意一个已选中范围内，则为选择
    canvas_mouse_state = 'select'

def onCanvasLeftButtonMove(event):
    global choose_box_canvas_id
    if canvas_mouse_state == 'select':
        if choose_box_canvas_id != -1:canvas.delete(choose_box_canvas_id)
        choose_box_canvas_id = canvas.create_rectangle(canvas_mouse_down_pos[0], canvas_mouse_down_pos[1],
            event.x, event.y)
    elif canvas_mouse_state == 'drag':
        global drag_box_canvas_ids
        deltaX = event.x - canvas_mouse_down_pos[0]
        deltaY = event.y - canvas_mouse_down_pos[1]
        # 清空拖动框
        for canvas_id in drag_box_canvas_ids:
            canvas.delete(canvas_id)
        # 更新拖动框位置
        for canvas_id in selected_canvas_ids:
            # 遍历找到该canvas_id对应的对象item
            for item in current_workspace['items']:
                if item['canvas_id'] == canvas_id:
                    break
            posNW = [item['position'][0] + deltaX,
                    item['position'][1] + deltaY]
            posSE = [item['position'][0] + item['rectangle_size'][0] + deltaX,
                     item['position'][1] + item['rectangle_size'][1] + deltaY]
            foo = canvas.create_rectangle(posNW[0]-BORDER_SIZE, posNW[1]-BORDER_SIZE,
                posSE[0]+BORDER_SIZE, posSE[1]+BORDER_SIZE, outline='green', width=BORDER_SIZE)
            drag_box_canvas_ids.append(foo)

# 按下Ctrl时可添加选中项
def onCtrlCanvasLeftButtonUp(event):
    global canvas_ctrl_down
    canvas_ctrl_down = True
    onCanvasLeftButtonUp(event)

def onCanvasLeftButtonUp(event):
    global canvas_mouse_state, selected_canvas_ids, canvas_ctrl_down
    if event.x == canvas_mouse_down_pos[0] and event.y == canvas_mouse_down_pos[1]:  # 若为点击,则一定为为选择
        canvas_mouse_state = 'select'
    # 动作为选择
    if canvas_mouse_state == 'select':
        if not canvas_ctrl_down: selected_canvas_ids = []
        # 根据框选或点击情况更新selected_canvas_ids
        # 选择方式为点击
        if abs(event.x-canvas_mouse_down_pos[0])<10 and abs(event.y-canvas_mouse_down_pos[1])<10:
            current_workspace['items'].reverse()  # 翻转
            for item in current_workspace['items']:  # 从后到前遍历寻找最上层的对象
                if _AInBRange([event.x, event.y], item['position'], item['rectangle_size']):
                    if item['canvas_id'] not in selected_canvas_ids:
                        selected_canvas_ids.append(item['canvas_id'])
                    else:
                        selected_canvas_ids.remove(item['canvas_id'])
                    break
            current_workspace['items'].reverse()  # 翻转
        # 选择方式为框选
        else:
            if choose_box_canvas_id != -1:canvas.delete(choose_box_canvas_id)  # 删除框选框
            for item in current_workspace['items']:  # 遍历寻找框内对象
                P_NW = item['position']
                P_SE = [item['position'][0] + item['rectangle_size'][0],
                        item['position'][1] + item['rectangle_size'][1]]
                # 两种情况，向右下拉或向左上拉
                # 向右下拉
                if event.x >= canvas_mouse_down_pos[0] and event.y >= canvas_mouse_down_pos[1]:
                    select_box_rectangle = [event.x - canvas_mouse_down_pos[0],
                                            event.y - canvas_mouse_down_pos[1]]
                    # 若对象完全在框选范围内
                    if _AInBRange(P_NW, canvas_mouse_down_pos, select_box_rectangle) and \
                       _AInBRange(P_SE, canvas_mouse_down_pos, select_box_rectangle):
                        if item['canvas_id'] not in selected_canvas_ids:
                            selected_canvas_ids.append(item['canvas_id'])
                # 向左上拉
                else:
                    # 选框左上在目标右下的左上，且选框右下在目标左上的右下
                    if event.x < item['position'][0] + item['rectangle_size'][0] and \
                       event.y < item['position'][1] + item['rectangle_size'][1] and \
                       canvas_mouse_down_pos[0] > item['position'][0] and \
                       canvas_mouse_down_pos[1] > item['position'][1]:
                        if item['canvas_id'] not in selected_canvas_ids:
                            selected_canvas_ids.append(item['canvas_id'])
    # 动作为拖动
    elif canvas_mouse_state == 'drag':
        deltaX = event.x - canvas_mouse_down_pos[0]
        deltaY = event.y - canvas_mouse_down_pos[1]
        # 对于被选中项,更改current_workspace中的position
        for canvas_id in selected_canvas_ids:
            # 遍历找到该canvas_id对应的对象item
            for item in current_workspace['items']:
                if item['canvas_id'] == canvas_id:
                    break
            item['position'] = [item['position'][0] + deltaX,
                                item['position'][1] + deltaY]
    canvas_mouse_state = 'up'
    canvas_ctrl_down = False
    UpdateCanvas()

def onCanvasRightButtonUp(event):
    canvas_menu.post(event.x_root, event.y_root) 

def UpdateCanvas():
    '''根据current_workspace和selected_canvas_ids更新画布'''
    global canvas, pics, selected_canvas_ids
    canvas.delete('all')
    _DrawFence()
    if len(current_workspace['items']) == 0: return  # 若无内容可不需执行后续动作
    pics = []  # 这个全局变量只是为了保证PhotoImage不被回收
    for item in current_workspace['items']:
        posNW = item['position']
        posSE = [item['position'][0]+item['rectangle_size'][0], 
                 item['position'][1]+item['rectangle_size'][1]]
        # 判断是否被选中
        if_selected = False
        item.setdefault('canvas_id',-1)
        if item['canvas_id'] in selected_canvas_ids:
            if_selected = True
            BORDER_SIZE = 2
            canvas.create_rectangle(posNW[0]-BORDER_SIZE, posNW[1]-BORDER_SIZE,
                posSE[0]+BORDER_SIZE, posSE[1]+BORDER_SIZE, outline='blue', width=BORDER_SIZE)
            selected_canvas_ids.remove(item['canvas_id'])  # 删掉旧的准备换新的
        # 画对象本身
        try:
            pic_org = Image.open('./calligraphy_lib/'+item['type']+'s/'+item['name']+'.png')
        except IOError:
            tkMessageBox.showinfo("错误","文件"+item['name']+'不存在, 已将其在工作区中自动删除', parent=root)
            current_workspace['items'].remove(item)
            continue
        pic_resized = pic_org.resize((int(item['rectangle_size'][0]), int(item['rectangle_size'][1])), Image.ANTIALIAS)  # 缩放至1像素1mm
        pic = ImageTk.PhotoImage(pic_resized)
        pics.append(pic)
        item['canvas_id'] = canvas.create_image(int(item['position'][0]), int(item['position'][1]), anchor=NW, image = pic)
        # 若之前被选中，重画后更新一下canvas_id
        if if_selected: selected_canvas_ids.append(item['canvas_id'])

def CoordTrans(paper_coord_points):
    # 坐标变换: 输入宣纸坐标系下的坐标轨迹，输出机械臂末端TCP对应的控制轨迹 [mm][rad]
    xyzrpy = [float(coord_x_entry.get()), float(coord_y_entry.get()), float(coord_z_entry.get()), 
              0, 0, float(coord_theta_entry.get())/180.0*pi]
    T_base2paper = xyzrpy2TransMartrix(xyzrpy)
    if len(paper_coord_points[0]) == 3:  # 数据不带rpy,直接在最后手动添加rpy
        # 宣纸坐标系->基座标系
        paper_coord_points = np.insert(paper_coord_points, 3, 1, axis=1)  # 齐次化
        base_coord_points = []
        for point in paper_coord_points:
            base_coord_points.append(T_base2paper.dot(point)[0:3])
        # 为所有点插入rpy
        base_coord_points = np.insert(base_coord_points, 3, pi, axis=1)  # r
        base_coord_points = np.insert(base_coord_points, 4, 0, axis=1)  # p
        base_coord_points = np.insert(base_coord_points, 5, float(coord_theta_entry.get())/180.0*pi, axis=1)  # y
    else:  # 数据带rpy,为在宣纸坐标系下的六轴坐标
        base_coord_points = []
        for point in paper_coord_points:
            T_paper2pen = xyzrpy2TransMartrix(point)
            T_base2pen = T_base2paper.dot(T_paper2pen)
            base_coord_points.append(TransMartrix2xyzrpy(T_base2pen))
    # 笔尖在基座标系下的姿态->默认TCP对应的姿态
    target_points = []
    for point in base_coord_points:
        TCP_pos = _End2TCP(point)
        target_points.append(TCP_pos)
    return target_points

def _SaveWorkspace(path):
    current_workspace['params']['coord_x'] = coord_x_entry.get()
    current_workspace['params']['coord_y'] = coord_y_entry.get()
    current_workspace['params']['coord_z'] = coord_z_entry.get()
    current_workspace['params']['coord_theta'] = coord_theta_entry.get()
    with open(path, 'w') as f:
        f.write(json.dumps(current_workspace, indent=4, ensure_ascii=False))
    return True

def _LoadWorkspace(path):
    global current_workspace, coord_x_entry, coord_y_entry, coord_z_entry, coord_theta_entry
    with open(path, 'r') as f:
        current_workspace = json.loads(f.read())
    coord_x_entry.delete(0,10)
    coord_x_entry.insert(0,current_workspace['params']['coord_x'])
    coord_y_entry.delete(0,10)
    coord_y_entry.insert(0,current_workspace['params']['coord_y'])
    coord_z_entry.delete(0,10)
    coord_z_entry.insert(0,current_workspace['params']['coord_z'])
    coord_theta_entry.delete(0,10)
    coord_theta_entry.insert(0,current_workspace['params']['coord_theta'])
    # 更新元素尺寸，因为可能做了修改，尺寸发生变化
    for item in current_workspace['items']:
        try:
            obj = json.loads(open('./calligraphy_lib/'+item['type']+'s/'+item['name']+'.json','r').read())
            if item['type'] == 'stroke':
                item['rectangle_size'] = obj['stroke_size']
            elif item['type'] == 'font':
                item['rectangle_size'] = obj['font_size']
        except IOError:
            tkMessageBox.showinfo("错误","文件"+item['name']+'不存在, 已将其在工作区中自动删除', parent=root)
            current_workspace['items'].remove(item)
            continue
    UpdateCanvas()
    return True

def _AInBRange(A=[0,0], B_NW=[0,0], B_rectangle=[1,1]):
    if B_NW[0] < A[0] and A[0] < B_NW[0] + B_rectangle[0] and \
       B_NW[1] < A[1] and A[1] < B_NW[1] + B_rectangle[1]:
        return True
    else:
        return False

def _DrawFence():
    global canvas
    x = CANVAS_WIDTH - FENCE_GAP
    while x > 0:
        canvas.create_line(x,0,x,CANVAS_HEIGHT,fill='black',dash=(10,10))
        x -= FENCE_GAP

def _BubbleSorting(item, _list):
    if _list == []:
        _list.append(item)
    else:
        for index, _item in enumerate(_list):
            if item['position'][1] < _item['position'][1]:
                _list.insert(index, item)
                return
        _list.append(item)  # 到最后都没能插入

def _PenShelfTrans(shelf_pos):
    '''笔架坐标系转基坐标系下TCP'''
    base2shelf = [config.PEN_SHELF_PARAMS[0], config.PEN_SHELF_PARAMS[1], config.PEN_SHELF_PARAMS[2], 
                  0, 0, config.PEN_SHELF_PARAMS[3]]
    T_base2shelf = xyzrpy2TransMartrix(base2shelf)
    T_shelf2pen = xyzrpy2TransMartrix(shelf_pos)
    T_base2pen = T_base2shelf.dot(T_shelf2pen)
    end_pos = TransMartrix2xyzrpy(T_base2pen)
    return _End2TCP(end_pos)

def _End2TCP(end_point):
    '''将笔尖在基座标系中的坐标转换为TCP在基坐标中的坐标'''
    T6to7 = xyzrpy2TransMartrix(config.BRUSH_PARAMS)
    T0to7 = xyzrpy2TransMartrix(end_point)
    T6to7_inv = np.linalg.inv(T6to7)
    T0to6 = T0to7.dot(T6to7_inv)
    return TransMartrix2xyzrpy(T0to6)

def _ArriveJudge(target_pos):
    '''判断是否到达,到达退出返回0,打断退出返回-1,(仅供取笔/放笔/蘸墨逻辑块使用)'''
    xyzWINDOW = 1  # mm
    rpyWINDOW = 0.02  # rad
    target_pos = np.array(target_pos)
    while True:
        if sum(abs(target_pos[0:3] - elfin_driver.TCP_pos[0:3])) < xyzWINDOW \
        and sum(abs(abs(target_pos[3:]) - abs(np.array(elfin_driver.TCP_pos[3:])))) < rpyWINDOW:
            time.sleep(0.2)  # 防止下条指令过快发出
            if writing_state != 'MOVING': return -1
            return 0
        if writing_state != 'MOVING': return -1
        time.sleep(0.1)


class AddItemWindow():
    '''用于向工作空间添加笔画或文字或动捕记录的弹窗 TODO:适配动捕记录'''
    def __init__(self, root, add_type):
        self.root = root
        self.type = add_type  # 'stroke'/'font'/'record'
        self.LABEL_BG = 180.0  # pix 背景矩形尺寸
        self.LABEL_SIZE = 160.0  # 笔画/文字预览尺寸
        self.LABEL_LINE_GAP = 40.0  # 两行间放名称的间隔行长度
        self.X_GAP = 200  # X方向一个对象的占位
        self.X_PADDING = int(self.X_GAP-self.LABEL_BG)/2  # X方向背景在占位两侧的padding
        self.Y_GAP = int(self.LABEL_BG+self.LABEL_LINE_GAP)  # X方向一个对象的占位
        self.chosen_index = None  # 一次选择的对象下标，再次点击则表示添加
        self.search_entry = None
        self.frame = tk.Frame
        self.items_name_list_total = []  # 所有item的名称list
        self.items_name_list = []  # 需要展示item的名称list
        self.label_list = []
        self.pic_list = []  # 这个用于存储最后用于画在canvas上的PhotoImage类型的对象，防止其作为局部变量被回收，canvas画img的大坑-_-|
        pic1 = Image.open('./misc/unselected.png')  
        pic2 = Image.open('./misc/selected.png')  
        self.unselected_img = ImageTk.PhotoImage(pic1)
        self.selected_img = ImageTk.PhotoImage(pic2)
        self.ReadItems()
        self.GUI_Init()
    
    def ReadItems(self):
        '''读取fonts或strokes下的预览图名称,存入self.items_name_list'''
        file_list = os.listdir('./calligraphy_lib/'+self.type+'s')
        for f_name in file_list:
            if re.search('.\.png', f_name) != None:
                self.items_name_list_total.append(f_name[:-4].decode('utf-8'))
        self.items_name_list_total.sort()  # 字节序排序
        self.items_name_list = self.items_name_list_total

    def GUI_Init(self):
        self.root.geometry("1000x940")
        if self.type=='stroke': title = '点击添加笔画(请点击笔画本身而非背景)'  
        elif self.type=='font': title = '点击添加文字(请点击文字本身而非背景)'
        elif self.type=='record': title = '点击添加记录(请点击文字本身而非背景)'
        self.root.title(title)
        self.root.bind('<Button-1>', self.OnLeftButtonDown)  # 用于判断选取的对象
        self.root.bind("<Return>", self.OnEnter)
        self.root.bind("<Escape>", self.OnEsc)
        # 搜索框
        tk.Label(self.root, text="搜索:", font=black12, fg='black').place(x=370, y=10)
        self.search_entry = tk.Entry(self.root, width='10', font=black12)
        self.search_entry.place(x=415, y=10)
        tk.Button(self.root, text="搜索", command=self.SearchCb, font=black12).place(x=525, y=5)
        self.UpdatePics()
    
    def UpdatePics(self):
        self.frame = tk.Frame(self.root)
        self.frame['width'] = 1000
        self.frame['height'] = 880
        self.frame.place(x=0, y=50)
        # 读取需要展示的预览图并渲染为label
        for index,name in enumerate(self.items_name_list):
            if index>=20: return  # 只显示前20个
            path = './calligraphy_lib/'+self.type+'s/'+name+'.png'
            pic_org = Image.open(path)  
            # 图片尺寸缩放
            resizeX = self.LABEL_SIZE
            resizeY = self.LABEL_SIZE
            if pic_org.height > pic_org.width:  # Y轴较长
                scale = self.LABEL_SIZE/pic_org.height
                resizeX = scale * pic_org.width
            else:  # X轴较长
                scale = self.LABEL_SIZE/pic_org.width
                resizeY = scale * pic_org.height
            pic_resized = pic_org.resize((int(resizeX), int(resizeY)), Image.ANTIALIAS)  # 缩放至预设尺寸
            self.pic_list.append(ImageTk.PhotoImage(pic_resized))
            # 预览图的排布
            x_index = index%5
            y_index = index/5
            tk.Label(self.frame, image=self.unselected_img).place(x=x_index*self.X_GAP+self.X_PADDING, y=y_index*self.Y_GAP)
            self.label_list.append(tk.Label(self.frame, image=self.pic_list[index]))
            if pic_org.height > pic_org.width:  # Y轴较长,X方向居中
                self.label_list[index].place(x=x_index*self.X_GAP+int((self.LABEL_BG-resizeX)/2), y=y_index*self.Y_GAP+10)
            else:  # X轴较长,Y方向居中
                self.label_list[index].place(x=x_index*self.X_GAP+20, y=y_index*self.Y_GAP+int((self.LABEL_BG-resizeY)/2))
            name = self.items_name_list[index]
            num = 0  # 记录名称中中文个数
            for c in name: 
                if 0x4E00 <= ord(c) <= 0x9FA5: num+=1
            x_offset = self.X_GAP/2- num*10 - (len(name)-num)*5
            tk.Label(self.frame, text=name, font=black14).place(x=x_index*self.X_GAP+x_offset, y=y_index*self.Y_GAP+182)
    
    def SearchCb(self):
        search_str = self.search_entry.get()
        self.items_name_list = []
        self.label_list = []
        self.pic_list = []
        self.chosen_index = None
        self.frame.destroy()
        for name in self.items_name_list_total:
            if re.search(search_str, name) != None:
                self.items_name_list.append(name)
        self.UpdatePics()
    
    def OnEnter(self, event):
        self.SearchCb()

    def OnEsc(self, event):
        self.root.destroy()

    def OnLeftButtonDown(self, event):
        global current_workspace,canvas
        # 获取当前点击的对象
        index = None
        if event.widget.widgetName != 'label':
            return
        else:
            for i,item in enumerate(self.label_list):
                if item == event.widget:
                    index = i
                    break
        if index == None:return  # 点到了背景图，而不是对象图本身
        if self.chosen_index != index:  # 点了新的
            # 恢复旧的对象背景图
            if self.chosen_index != None:  # 第一次点击不需更新
                old_index = self.chosen_index
                x_index = old_index%5
                y_index = old_index/5
                tk.Label(self.frame, image=self.unselected_img).place(x=x_index*self.X_GAP+self.X_PADDING, y=y_index*self.Y_GAP)
                pic_size = self.pic_list[old_index]._PhotoImage__size
                # 创建一个新的Label,防止旧Label被覆盖
                self.label_list[old_index].destroy()
                self.label_list[old_index] = tk.Label(self.frame, image=self.pic_list[old_index])
                if pic_size[1] > pic_size[0]:  # Y轴较长,X方向居中
                    self.label_list[old_index].place(x=x_index*self.X_GAP+int((self.LABEL_BG-pic_size[0])/2), y=y_index*self.Y_GAP+10)
                else:  # X轴较长,Y方向居中
                    self.label_list[old_index].place(x=x_index*self.X_GAP+20, y=y_index*self.Y_GAP+int((self.LABEL_BG-pic_size[1])/2))
            # 更新新的对象背景图
            self.chosen_index = index
            x_index = index%5
            y_index = index/5
            tk.Label(self.frame, image=self.selected_img).place(x=x_index*self.X_GAP+self.X_PADDING, y=y_index*self.Y_GAP)
            pic_size = self.pic_list[index]._PhotoImage__size
            self.label_list[index].destroy()
            self.label_list[index] = tk.Label(self.frame, image=self.pic_list[index])
            if pic_size[1] > pic_size[0]:  # Y轴较长,X方向居中
                self.label_list[index].place(x=x_index*self.X_GAP+int((self.LABEL_BG-pic_size[0])/2), y=y_index*self.Y_GAP+10)
            else:  # X轴较长,Y方向居中
                self.label_list[index].place(x=x_index*self.X_GAP+20, y=y_index*self.Y_GAP+int((self.LABEL_BG-pic_size[1])/2))
        else:
            # 将笔画/文字/记录加入画布,更新current_workspace
            name = self.items_name_list[self.chosen_index]
            obj = json.loads(open('./calligraphy_lib/'+self.type+'s/'+name+'.json','r').read())
            rectangle_size = obj[self.type+'_size']
            foo = {
                'name':name,
                'position':[(CANVAS_WIDTH-rectangle_size[0])/2, (CANVAS_HEIGHT-rectangle_size[1])/2],
                'rectangle_size':[rectangle_size[0],rectangle_size[1]],
                'type':self.type,
            }
            current_workspace['items'].append(foo)
            UpdateCanvas()


if __name__ == "__main__":
    GUI_Init()
